library IEEE;
use IEEE.std_logic_1164.all;
use IEEE.numeric_std.all;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;

entity CORDIC is 
	port( 
			Xin		 : in std_logic_vector(34 downto 0); 
		   Yin			 :	in std_logic_vector(34 downto 0);
		   Zin			 : in std_logic_vector(28 downto 0); 
		   Xout		 : out std_logic_vector(34 downto 0);
		   Yout		 : out std_logic_vector(34 downto 0)
	);
end CORDIC;

-- Las entradas entran en complemento a 2

architecture cordic of CORDIC is
begin
	process(Xin,Yin,Zin)
	    TYPE std_logic_matrix29 IS ARRAY (NATURAL RANGE <>) OF std_logic_vector(28 downto 0);
	    TYPE std_logic_matrix35 IS ARRAY (NATURAL RANGE <>) OF std_logic_vector(34 downto 0);
	    constant at: std_logic_matrix29 (0 to 6):=(std_logic_vector(to_signed(45000000,29)),std_logic_vector(to_signed(26565051,29)),std_logic_vector(to_signed(14036243,29)),std_logic_vector(to_signed(7125016,29)),std_logic_vector(to_signed(3576334,29)),std_logic_vector(to_signed(1789911,29)),std_logic_vector(to_signed(895174,29)));
	    constant ganancia: std_logic_vector(26 downto 0):=std_logic_vector(to_signed(60735177,27));
	    variable Xaux: std_logic_matrix35 (0 to 7);
	    variable Yaux: std_logic_matrix35 (0 to 7);
	    variable Zaux: std_logic_matrix29 (0 to 7);
	    variable Prod: std_logic_vector (61 downto 0);
	begin
	        Xaux(0):=Xin;
	        Yaux(0):=Yin;
	        Zaux(0):=Zin;
	        if Zaux(0)(0)='0'  then --d=1
	              Xaux(1):=Xaux(0)-Yaux(0);
	              Yaux(1):=Yaux(0)+Xaux(0);
	              Zaux(1):=Zaux(0)-at(0);
	        else --d=-1
	              Xaux(1):=Xaux(0)+Yaux(0);
	              Yaux(1):=Yaux(0)-Xaux(0);
	              Zaux(1):=Zaux(0)+at(0);
	        end if;
	        for i in 1 to 6 loop
	           if Zaux(i)(0)='0' then --d=1
	              Xaux(i+1):=Xaux(i)-((34 downto 35-i => Yaux(i)(34))&(Yaux(i)(34 downto i)));
	              Yaux(i+1):=Yaux(i)+((34 downto 35-i => Xaux(i)(34))&(Xaux(i)(34 downto i)));
	              Zaux(i+1):=Zaux(i)-at(i);
	           else --d=-1
	              Xaux(i+1):=Xaux(i)+((34 downto 35-i => Yaux(i)(34))&(Yaux(i)(34 downto i)));
	              Yaux(i+1):=Yaux(i)-((34 downto 35-i => Xaux(i)(34))&(Xaux(i)(34 downto i)));
	              Zaux(i+1):=Zaux(i)+at(i);
	           end if;
	        end loop;
	        Prod:=ganancia*Xaux(7);
	        Xout<=Prod(61 downto 27);
	        Prod:=ganancia*Yaux(7);
	        Yout<=Prod(61 downto 27);
	end process;
end cordic;